#!/usr/bin/python3
#coding:utf-8
import time
import threading
import os
import socket
class NANO:
    def __init__(self):

        self.vision_run = True
        self.vision_thread = threading.Thread(target = self.init_vision_func, args=())

        self.init_udp()
        self.udp_run = True
        self.udp_status = 0
        self.udp_thread = threading.Thread(target = self.receive_udp_func, args=())
    
    def init_vision_func(self):
        self.cmd = "bash -c "\
            "'LD_PRELOAD=~/robotcup_ws/Yolov5-in-Deepstream-5.0/Deepstream_5.0/libmyplugins.so "\
            " deepstream-app -c "\
            "~/robotcup_ws/Yolov5-in-Deepstream-5.0/Deepstream_5.0/deepstream_app_config_yoloV5.txt'"
        os.system(self.cmd)

    def init_udp(self):
        self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.localaddr = ("127.0.0.1",8888) # 必须绑定自己电脑IP和port
        self.udp_socket.bind(self.localaddr)
    
    def receive_udp_func(self):
        while self.udp_run:
            res,_ = self.udp_socket.recvfrom(1024)    
            if res == '':
                continue
            res = res.decode("utf8")
            status = 0
            if status == 0:
                if res[0:2] == 'AA':
                    status = 1
                else:
                    status = 0
            if status == 1:
                data_len = int(res[2:4])
                # print("data_len:",data_len)
                status = 2
            if status == 2:
                for i in range(data_len):
                    temp = 4+i*18
                    data_x = int(res[temp:temp+4])
                    data_y = int(res[temp+4:temp+8])
                    data_w = int(res[temp+8:temp+12])
                    data_h = int(res[temp+12:temp+16])
                    class_id = int(res[temp+16:temp+18])
                    print(data_x,data_y,class_id)
            status = 0
            time.sleep(1/50)



    def start(self):
        self.vision_thread.start()
        # self.serial_thread.start()
        self.udp_thread.start()
            

if __name__ == '__main__':
    node1 = NANO()
    node1.start()
